#pragma once
#include "Matrix44.h"

const double S_CATNULL = 0.5;

template <class T,typename V>
class Spline
{
public:
	Spline()
	{
		m_hermitMatrix.m[0] = 1;m_hermitMatrix.m[1] = 0;m_hermitMatrix.m[2] = -3;m_hermitMatrix.m[3] = 2;
		m_hermitMatrix.m[4] = 0;m_hermitMatrix.m[5] = 0;m_hermitMatrix.m[6] = 3 ;m_hermitMatrix.m[7] = -2;
		m_hermitMatrix.m[8] = 0;m_hermitMatrix.m[9] = 1;m_hermitMatrix.m[10]= -2;m_hermitMatrix.m[11] = 1;
		m_hermitMatrix.m[12]= 0;m_hermitMatrix.m[13]= 0;m_hermitMatrix.m[14]= -1;m_hermitMatrix.m[15] = 1;

		m_catnullMatrix.m[0] = 0;m_catnullMatrix.m[1] = 0;m_catnullMatrix.m[2] = -S_CATNULL;m_catnullMatrix.m[3] = 0;
		m_catnullMatrix.m[4] = 1;m_catnullMatrix.m[5] = 0;m_catnullMatrix.m[6] = 0;m_catnullMatrix.m[7] = -S_CATNULL;
		m_catnullMatrix.m[8] = 0;m_catnullMatrix.m[9] = 1;m_catnullMatrix.m[10] = S_CATNULL;m_catnullMatrix.m[11] = 0;
		m_catnullMatrix.m[12] = 0;m_catnullMatrix.m[13] = 0;m_catnullMatrix.m[14] = 0;m_catnullMatrix.m[15] = S_CATNULL;
	}

	~Spline(void)
	{}

	void setInput(const V& p0,const V& v0,const V& p1,const V& v1,int type=0);
	void dump() const;

	V getPt() const;
	void computeHermit(T t, V& p, V& v);
	void computeCatnull(T t, V& p, V& v);
	void getBlendFunction(Vector4<T>& p,Vector4<T>& v) {p = m_hermitP,v = m_hermitV;}
private:
	//void computeHermit();
	//void computeCatnull();
private:
	Matrix44<T> m_transformMatrix;
	Matrix44<T> m_hermitMatrix;
	Matrix44<T> m_catnullMatrix;
	Vector4<T> m_hermitP;
	Vector4<T> m_hermitV;
	T m_t;
	double m_deltat;
	V m_p0,m_v0,m_p1,m_v1,m_p2,m_p3;
};

template <class T,typename V>
void Spline<T, V>::setInput( const V& p0,const V& p1,const V& p2,const V& p3,int type=0 )
{
	if (type==0)
	{
		m_p0 = p0;
		m_p1 = p1;
		m_p2 = p2;
		m_p3 = p3;
	}
	else
	{
		m_p0 = p0;
		m_p1 = p1;
		m_v0 = p2;
		m_v1 = p3;
	}
}

template <class T,typename V>
void Spline<T, V>::computeHermit( T t, V& p, V& v )
{
	m_hermitP = m_hermitMatrix*Vector4<T>(1,t,t*t,t*t*t);
	m_hermitV = m_hermitMatrix*Vector4<T>(0,1,2*t,3*t*t);

	p = m_p0*m_hermitP.x+m_p1*m_hermitP.y+m_v0*m_hermitP.z+m_v1*m_hermitP.w;
	v = m_p0*m_hermitV.x+m_p1*m_hermitV.y+m_v0*m_hermitV.z+m_v1*m_hermitV.w;
}

template <class T,typename V>
void Spline<T, V>::computeCatnull( T t, V& p, V& v )
{
	Matrix44<T> matrix = m_catnullMatrix*m_hermitMatrix;
	m_hermitP = matrix*Vector4<T>(1,t,t*t,t*t*t);

	p = m_p0*m_hermitP.x+m_p1*m_hermitP.y+m_p2*m_hermitP.z+m_p3*m_hermitP.w;
}